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Abstract — Formations  that  contain  a  small  number  of 
robots  are  modeled  as  controlled  Lagrangian  systems  on 
Jacobi  shape  space.  This  allows  a  block-structured  control  of 
position,  orientation  and  shape  of  the  formation.  Feedback 
control  laws  are  derived  using  control  Lyapunov  functions. 
The  controlled  dynamics  converges  to  the  invariant  set  where 
desired  shape  is  achieved.  Controllers  are  implemented 
in  a  layered  fashion  via  the  extended  motion  description 
language(MDLe)  system.  Group  MDLe  plans  are  constructed 
to  allow  structured  controller  design  for  formations. 

I.  Introduction 

Robots  in  a  formation  can  be  viewed  as  physical  objects 
with  the  control  effort  viewed  as  interaction.  Hence  a  robot 
formation  can  be  modeled  as  a  controlled  Lagrangian 
system  of  particles.  What  are  the  interactions  which  allow 
particles  (robots)  to  form  a  meaningful  (stable)  formation 
is  the  problem  we  want  to  investigate. 

The  shape  of  a  formation  is  invariant  under  translation 
and  rotation.  It  is  independent  of  the  coordinate  system 
we  choose  to  study  the  whole  formation.  Jacobi  proposed 
a  special  class  of  coordinates  ([7],  [2],  [12])  which  served 
as  the  starting  point  to  what  we  call  the  Jacobi  shape 
space.  On  this  shape  space,  the  global  displacements  (of 
translation  and  rotation)  are  not  present.  This  shape  space 
concept  will  be  discussed  in  section  II.  It  is  important 
to  robot  formation  control  since  without  knowing  robot 
coordinates  in  the  laboratory-fixed  coordinate  system,  only 
shape  variables  can  be  measured  using  on-board  sensors 
of  the  robots.  Ideally,  the  control  laws  to  achieve  desired 
shape  should  only  depend  on  shape  measurements.  How¬ 
ever,  as  suggested  by  the  form  of  the  Lagrange  equations 
in  section  III,  it  is  impossible  to  achieve  completely 
decoupled  control  of  shape  and  orientation.  In  section 
IV,  we  design  shape  controllers  using  control  Lyapunov 
functions.  An  estimate  of  the  angular  velocity  of  the 
whole  formation  will  greatly  simplify  the  task.  We  are 
able  to  control  the  formation  to  desired  shape  with  fixed 
orientation. 


One  can  avoid  implementing  a  formation  controller  on 
each  robot  explicitly  by  conceptualizing  a  hierarchical 
approach.  The  block-structured  control  law  of  section  IV 
permit  such  a  view  point.  The  control  of  overall  position 
and  orientation  can  be  viewed  as  group  level  commands. 
This  inspired  us  to  add  group  features  to  an  existing 
unified  platform  called  the  extended  motion  description 
language  (MDLe).  This  work  is  presented  in  section  V. 


II.  Jacobi  shape  space 


To  describe  the  motion  of  a  cluster  of  particles,  we  set 
up  a  fixed  inertial  coordinate  frame  first.  Let  qi  £  , 

i  =  1,2,,.., IV,  denote  the  coordinates  of  N  particles  with 
mass  nq.  The  kinetic  energy  of  this  cluster  is 

x**  =  lLmiM2  a) 

z  1=1 


This  kinetic  energy  is  translation  invariant.  Let  M  = 
Denote  the  center  of  mass  as 


qc~  M 


(2) 


Now  one  can  define  a  new  set  of  coordinates  c,-  as  c/(-  = 
qi  —  qc  .  Then  the  kinetic  energy  can  be  expressed  as 


(3) 


But  since  Yli=  \  micn  :  0-  we  seel<  (A/  —  1 )  indepen¬ 
dent  vectors  (p.,f  =  1,2 1)  from  s panic,-,  i  = 
1,2, ..., N).  We  want  p to  be  chosen  such  that  the  kinetic 
energy  has  the  form 

Ktot  =  \M\\qc\\2 + 

z  z  1=1 


Such  a  set  of  p  ,.  are  called  Jacobi  coordinates.  One  way 
of  constructing  Jacobi  coordinates  is  to  let 


P/i 

P/2 


\^T(C/2 

VTh.(c/3 


c/i) 

mlcfl+m2cf2^ 

ml  +m2 


Pfi  —  \fPi(Cf(i+\)  ' 
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P/(Af-l)  —  \/pN-l(CfN 
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where 


1 

Pi 
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fori  =  1,2,...,#-  1  (6) 


LLi"L  '»;+i 

A  proof  for  this  well-known  construction  satisfying  (4) 
can  be  found  in  [22](see  also  discussion  in  [12]).  As 
one  can  see  from  these  equations,  the  vectors  p are 
constructed  by  finding  the  scaled  relative  displacement 
between  the  (7+  l)th  particle  and  the  center  of  mass  of  the 
sub-cluster  of  first  i  particles.  This  process  depends  on  how 
the  particles  are  labeled.  We  can  also  change  the  way  we 
sub-cluster  particles  as  in  [2],  [11]  and  [12].  Hence  Jacobi 
coordinates  are  not  unique.  However,  between  any  two  sets 
of  Jacobi  coordinates  there  exists  an  element  h  £  0(N  —  1) 
s.t 


[P/I,P/2,-,P/(JV— !)]  =  \Pfl,Pf2,-,Pf(N-l)]h 

This  orthogonal  group  0(N  —  1)  is  called  the  democracy 
group  [12]. 

Let  Q  =  be  the  total  configuration  space  of  the 
formation.  The  space  of  Jacobi  coordinates  is  A? 3A?  3 .  We 
define 


This  K  is  invariant  under  the  diagonal  left  action  on  /X?3,v  3 
by  the  special  orthogonal  group  G  =  5(9(3).  The  action  is: 

3*g  (Pfi)  =  SPfi  for  g£  G  (9) 

This  symmetry  group  G  acts  on  ;7f3  v  3  properly  and 
freely  except  for  the  shapes  where  all  p  ,.  are  collinear. 
We  let  the  set  F0  be  the  set  of  all  the  Jacobi  coordinates 
corresponding  to  collinear  shapes.  Let  F  =  /773,v  3  —  F() 
and  call  it  the  Jacobi  pre-shape  space.  It  is  an  open 
submanifold  of  the  configuration  space.  Since  G  acts 
properly  and  freely  on  F ,  the  base  space  B  =  F/G  is  a 
smooth  manifold  and  the  canonical  projection  7T  :  F  — ►  B 
is  differentiable.  B  is  called  the  Jacobi  shape  space. 

In  dropping  from  F  to  B ,  we  get  rid  of  the  5(9(3)  sym¬ 
metry  from  the  Jacobi  coordinates.  After  the  reduction, 
the  dimension  of  the  shape  space  B  is  (3N  —  6).  On  this 
shape  space  we  can  define  shape  coordinates  s 7  as 

sJ  =sJ(pfv pf2 ,..., p/(JV_ j } )  for  j  =  1 , 2 , . . . (3 N - 6 )  (10) 
s.t. 


si(gPf\igPf2i -—>gPf(N- 1))  —  sj(PfvPf2 >  ■■■■>Pf<N- 1)) 

(11) 


for  all  g  £  5(9(3).  Candidates  for  sJ  are  functions  of  dot 
products  ( p ^  ■  p^)  and  triple  products  (pj-  •  ( pj-  x  p^,)). 
Thus,  mutual  distances,  mutual  angles,  areas  and  volumes 
formed  by  the  line  segments  connecting  the  particles  all 
serve  as  candidates  for  shape  variables.  There  is  a  large 
statistical  literature  on  the  subject  of  shape  space  and 
shape  coordinates) [8]  [9]  [19]). 

One  can  establish  a  body  coordinate  system  on  a 
formation  with  certain  shape.  The  reference  orientation 
of  this  formation  can  be  defined  as  the  orientation  when 
the  body  coordinate  frame  and  the  lab  coordinate  frame 
coincide.  Then  the  orientation  of  this  formation  with  the 
same  shape  can  be  described  by  an  element  g  £  5(9(3). 
The  Jacobi  coordinates  in  these  two  coordinate  systems 
have  the  following  relationship: 

Pfi  =  8Pi(s)  (12) 

where 

s  =  {s\s\...,s™-0f.  (13) 

p j  are  Jacobi  coordinates  in  the  body  coordinate  frame 
which  only  depend  on  shape  coordinates. 

Taking  derivative  on  both  sides  of  (12),  we  get 

Pfi  =  gPi  +  gPi  (14) 

On  a  matrix  Lie  group  G,  g  £  Tg G.  There  exists  Q.  £  g 
the  Lie  algebra  s.t.  g  =  gCl  .  In  our  case,  G  =  5(9(3),  so 
0=  (^3  ,  x).  Thus  the  derivative  of  p is 

™-6  do 

pfi=g(Clxpi+  £  (15) 

In  the  body  coordinate  frame,  the  angular  momentum  of 
the  whole  system  J  can  be  calculated  as 

N- 1 

J  =  g~l  £  (pp  x  Pfi) 

i=  1 
3N-6 

=  I(Q.+  £  A/)  (16) 

i=  1 

where 

KllPtlfe-Pipf)  (17) 

;=i 

is  defined  as  the  locked  inertia  tensor  of  the  formation  in 
the  body  coordinate  frame  and 

Ai(,)=/“1£1p,.x|5  (18) 

are  vector  potential  functions.  These  quantities  are  defined 
on  the  shape  space  because  p(  only  depend  on  shape 
coordinates. 

Let 

A=[AX  ,A2,...,A3Jv-6]  (19) 


Then  we  can  rewrite  the  kinetic  energy  in  block  diagonal¬ 
ized  form  as 


1 


1 


K‘ot  =  \m  I!  qc  ||2  +  +  As)1 7(Q+ As)  +  ^s1  Gs  (20) 


where 


fit 


(21) 


In  our  paper  in  preparation  [22],  a  geometrically  in¬ 
trinsic  approach  will  be  taken  for  the  above  construction 
following  the  theory  of  block  diagonalization  as  in  [16], 
[17],  [18],  [10],  [21], 

III.  Lagrange  equations  for  formations 

By  defining  Jacobi  vectors  and  then  Jacobi  shape  vari¬ 
ables  we  have  gone  through  a  sequence  of  changes  of 
coordinates  on  the  configuration  space  Q.  In  the  first  step, 
the  transformation  is  a  diffeomorphism  between  Q  and 
R?n  mapping  configuration  variables  qi  to  the  position  of 
the  center  of  mass  qc  and  the  Jacobi  vectors  p .  In  the 
second  step,  the  transformation  is  a  local  diffeomorphism 
between  R3  x  F  and  Rl  xG//i  mapping  (qc,Pfj)  where 
i  =  1,2 1)  to  (, qc,g,sj )  where  j=  1,2, ...(3IV- 6) 
s.t. 

Pfi  =  gPi(s1,s2,-s3N~6)  (22) 

The  Lagrangian  on  TQ  is  given  by  L(q,q)  =  Klot(q)  — 

V(q)  • 

Before  the  coordinate  transformation,  K""  has  the  form 
in  equation  (1).  The  Lagrange  equations  for  the  system 
are: 

dV 

mqi  =  u-  -  ——  (23) 

dq{ 

for  ;  =  1,2,  ...,1V  where  ui  are  control  forces. 

After  the  block  diagonalization,  Ktot  is  given  by  equa¬ 
tion  (20),  and  one  can  rewrite  the  Lagrangian  in  block 
diagonalized  form  using  A  and  G  as 

1 


L(qClg,s,qc,g,s)  = 


\M\\qc\\2 

1  T 

+  -s  Gs  —  V (qc,g,s,qc) 


(O  -\-As)J  1(0  +  As) 


(24) 


Now 


dL 

dqc 

dL 

dL 


Mqc 

g/(f2  +  Ai) 

AtI(Q.+As)- 


Gs 


(25) 


Then  the  set  of  Lagrange  equations  takes  the  form 

dV 


Mqc  =  - 


dqc 


jtm+A £)) 


Q.  x  I(Cl  +  As)  —  g 


dV 

"X - f  Up 

dg  8 


(27) 


-j-(Gs)  +  AtL/(Q+As))=^]*:(Q  +  As,Q  +  As) 
at  at  2  ds 

+\&{s,s)~d-L  +  us  (28) 

2  ds  ds 

In  these  equations,  [  and  [^]  are  three-tensors  obtained 
by  taking  the  Frechet  derivatives  of  the  matrix  I,G  with 
respect  to  vector  s.  [  J^]*  and  [^p]*  are  the  cyclic  transpose 
of  these  three  tensors  c.f.  [20]  and  [1], 

The  relationship  between  the  control  forces  ui  and 
( uClug,us ),  comes  from  a  well  known  fact  for  controlled 
Lagrangian  system.  If  the  coordinate  transformation  r  = 
r(q)  is  a  local  diffeomorphism,  then 

drT 

ur  =  (—)~luq.  (29) 

Therefore,  from  equation  (5),  we  can  calculate 
the  relations  between  (ul,u2,---,uj,.tLun)  and 

as: 


N- 1 


•  'L;  _  ri 

=  MUc  +  VFiufi-mi  L 


Afj 


J=l 


mx  +m2-\ - \ -rrtj 


■  (30) 


for  i  =  1,2, ..., (N  —  1).  Then  by  using  the  same  method. 


ug  and  us  can  be  solved  from 


N-  1 


=  T,pjxs 


i=i 


fj 


N~ 1  dp 

=  E(p7)V‘- 

i=i  ask 


'n  ■ 


(31) 


where  k=  1,2,..., (31V  — 6). 


IV.  Feedback  control  using  shape 
measurements 

Suppose  the  potential  function  V  is  invariant  under 
translation  and  rotation  i.e.  V  is  only  a  function  of  the 
shape  variables.  Then  in  the  system  equations  (26),  (27) 
and  (28),  the  equation  for  qc  is  decoupled  from  the  other 
two.  Then  we  can  define  a  function  on  the  tangent  bundle 
of  the  pre-shape  space  as 

VL=  ^  ||.?-s0||2  +  i(f2+As)7’/(f2  +  Ai)  +  isrGi  (32) 

where  s°  specifies  a  desired  shape.  The  derivative  of  this 
function  along  the  reduced  dynamics  (27)  and  (28)  is 

VL  =  <  (s  —  s°),s  >  +  <  Q:ug  >  +  <  s,us—  > 

=  <s,u? — =; — t-  (s  —  s°)  >  +  <  Q.,Ug  >  (33) 

ds 


(26) 


where  we  use  <  ,  >  to  denote  the  inner  product.  Hence 
by  letting  the  control  law  be 

us  =  —k  j£2 

us  =  ^-(s-s°)-s  (34) 

ds 

where  kx  >0,  we  have 

Vi.  =  -IMI2-Mfl||2<0  (35) 

We  know  that  on  the  tangent  bundle  TF  of  Jacobi  pre¬ 
shape  space  F,  V L  is  radially  unbounded.  So  we  can  apply 
LaSalle’s  invariance  principle  to  argue  that  the  controlled 
dynamics  converge  to  the  maximal  invariant  set  C2  within 
the  set  /V/-,  where  VL  =  0.  Hence 

M2  =  {{gls,Tl,s)eTF\Tl  =  0,s=0}  (36) 

and 

C2  =  {C?’S’^)  GM2|D  =  0,i'=0}  (37) 

In  the  system  equations  (27)  and  (28),  letting  Cl  =  0  and 
s  =  0,  we  have 

m  =  ug  =  0 

dv 

Gs  +  IQ.  =  Us  — ~ —  =  0  (38) 

ds 

Thus  on  the  set  C2,  in  order  for  Cl  =  0  and  s  =  0,  we 
must  have  s  —  s°  =  0  .  Thus  we  have  proved  the  following 
theorem 

Theorem  4.1:  Suppose  the  potential  V  is  rigid  motion 
invariant.  By  using  the  feedback  control  law  (34),  the 
Jacobi  shape  ,v°  is  locally  asymptotically  stablized. 

However,  by  letting  ug  =  —kxCl  we  already  made  the 
assumption  that  Cl  can  be  measured.  In  fact,  Cl  need  not 
to  be  measured  accurately.  All  we  need  is  an  estimate  of 
the  direction  of  £2  which  will  ensure  i  £1  be  decreasing. 
This  estimate  can  not  be  obtained  by  only  measuring  ,v 
and  s.  Some  extra  sensors  need  to  be  employed  which 
will  observe  the  relative  movements  of  fixed  landmarks 
with  respect  to  the  formation. 

V.  Group  MDLe 

Motion  description  language  (MDL)  was  first  developed 
as  a  setting  for  robot  programming  in  [4], [3], [5],  In  the 
enhanced  form  MDLe,  it  provided  a  formal  basis  for 
robot  programming  using  behaviors  and  at  the  same  time 
permitted  incorporation  of  kinematic  and  dynamic  models 
of  robots  in  the  form  of  differential  equations.  In  the  work 
of  Manikonda,  Krishnaprasad  and  Hendler  ([14], [13]),  the 
idea  of  introducing  sensor-triggered  interrupts  as  elements 
of  an  enhanced  MDL  was  introduced.  In  the  paper  [15],  a 
comprehensive  overview  of  these  developments  on  MDLe 
is  given.  A  recent  implementation  of  MDLe  is  described 
in  [6], 


In  MDLe,  the  physical  system  is  modeled  as  a  so-called 
kinetic  state  machine.  As  defined  in  [15],  a  kinetic  state 
machine  is  governed  by  a  differential  equation  of  the  form: 

x  =  f(x)  +  G(x)U ;  y  =  h(x)  G  (39) 

where  x(-)  :  — >  !%n,  U(-)  :  t%+  x  S%n  — >  8%m  is  a  time 

dependent  control  law  and  G  is  a  matrix  whose  columns 
g-  are  vector  fields  in  M" .  The  smallest  building  block 
of  MDLe  is  called  an  atom.  An  atom  is  a  triple  of  the 
form  a  =  (U,  q .  T).  where  U  is  the  control  defined  before, 
q  :  — >  {0, 1}  is  a  boolean  interrupt  function  defined  on 

the  space  of  outputs  from  p  sensors,  and  7  £  denotes 
the  time  (measured  from  the  instance  an  atom  is  activated) 
at  which  the  atom  will  “time  out”.  To  evaluate  the  atom 
cr  means  to  apply  the  input  U  to  the  kinetic  state  machine 
until  the  interrupt  function  £,  is  triggered  or  until  T  units 
of  time  elapse,  whichever  occurs  first.  We  note  that  T 
is  allowed  to  be  °°.  The  input  u  could  be  an  open  loop 
command  or  could  be  given  by  a  feedback  law  of  the  type 
u  =  it(t.x).  In  our  implementation,  atoms  can  be  assembled 
using  quarks.  A  quark  is  a  piece  of  code  which  is  reusable 
by  different  atoms.  For  instance, 

( Atom  ( bumper )  (go  30  30)) 

defines  an  atom  to  drive  a  robot  forward  at  speed  30  until 
it  bumps  into  an  obstacle.  This  will  trigger  the  bumper 
sensors  to  interrupt  the  atom.  Here,  “bumper”  and  “go” 
are  quarks  which  can  be  shared  by  other  atoms. 

To  implement  the  formation  control  law,  we  define  a 
new  concept  called  the  group  kinetic  state  machine  by 
modifying  the  form  of  equation  (39)  as 

*i  =  fi(Xi)  +  Gfi(,Xi)Ufi  +  Ggi(XVX2’"-’XN)Ug:’ 

yt  =  h(xt)  G  (40) 

where  the  index  i  denotes  the  ith  robot.  LG  represents  the 
control  laws  that  will  achieve  the  formation  stablization  or 
bring  about  changes  in  shape  and  size  of  the  formation. 
UH  represents  the  group  level  control  which  will  treat  the 
whole  group  as  one  robot.  As  is  obvious,  we  separated 
controls  achieving  formations  from  controls  that  treat  the 
formation  as  a  unity.  With  respect  to  this  separation,  we 
define  a  group  atom  to  be  the  triple  (Ug,^g,T)  where  <;„ 
is  defined  as  the  group  level  interrupt  which  depends  on 
the  sensor  information  of  all  the  robots.  A  shadow  atom  is 
a  modification  of  a  group  atom.  It  is  defined  as  (f/;,  <L,  T) 
where 

Ui  =  Ufi  +  Tii;  Gfj(xi)rii  =  Ggi(xx,x2,...,xN)Ug  (41) 
and  (L  is  a  function  of  (yq.^g).  This  will  result  in 

xt  =  fi(xt )  +  Gfi(xi)ui ;  yt  =  Kx,)  e  (42) 

which  agree  with  the  kinetic  state  machine  representation 
of  a  single  robot.  The  mapping  from  group  atoms  to 


shadow  atoms  will  allow  formation  keeping  controllers  to 
take  effect.  Mappings  can  be  written  a  priori  by  designing 
proper  quarks.  Thus  all  shadow  atoms  can  share  a  small 
set  of  mapping  quarks.  For  instance, 

(i GroupAtom  ( Bumper )  (go  30  30)) 

is  a  group  atom  which  will  drive  a  group  of  robots  forward 
until  any  of  them  bumps  into  an  obstacle.  In  order  to  keep 
all  the  robots  in  formation,  assume  one  of  the  robots  is 
lagging  behind  its  nominal  position,  the  shadow  atom  on 
this  robot  might  be 

(Atom  ( Bumper )  (go  31  31)) 

where  the  number  “31”  is  obtained  by  a  mapping  quark 
where  formation  control  is  implemented. 

A  set  of  atoms  can  be  composed  into  a  string  with  its 
own  interrupt  function  and  timer.  Such  strings  are  called 
behaviors.  Behaviors  themselves  can  be  used  to  form 
higher-level  structures  (partial  plans)  which  in  turn  can  be 
nested  into  plans.  MDLe  allows  for  arbitrarily  many  levels 
of  nested  atoms,  behaviors  and  plans.  The  behaviors  and 
plans  consisting  purely  of  group  atoms  are  called  group 
behaviors  and  group  plans.  Group  plans  are  shared  by 
all  the  robots  in  a  formation. 

To  illustrate  the  usage  of  MDLe  for  formation  control, 
we  show  an  MDLe  script  that  will  command  a  group 
of  two  mobile  robots  to  wander  around  without  hitting 
obstacles  while  keeping  their  initial  relative  positions, 
main  =  (ExecPlan  1  (nop) 
robotOn 

(GroupConnect  2  12346  12356  sulu:  12345  sulu:  12355) 
(GroupPlan  -1  (or  (not  (areclientsconnected  1))  (bumper)) 
(GroupAtom  frontObstacle  (go  30  30)) 

(GroupAtom  (GroupClear  20  15  10)  (go  -50  50))) 
robotOff); 

Here,  “robotOn”  and  “robotOff"  are  quarks  that 
will  turn  on  and  off  the  robot.  “GroupConnect”  is  a  quark 
which  will  terminate  after  it  connects  to  every  other  robot 
in  the  group.  Its  arguments  include  the  ports  this  robot 
should  listen  on  for  connections  and  the  addresses  of  the 
robots  it  will  connect  to.  The  “2”  is  the  number  of  robots 
in  the  group  and  “sulu”  is  the  name  of  the  other  robot. 
Note  that  in  this  example,  we  are  using  TCP/IP  sockets 
for  inter-robot  communications. 

The  group  plan  specifies  a  plan  level  interrupt  that  will 
stop  the  entire  plan  from  executing  if  either  the  bumper 
is  hit,  or  if  one  of  the  robots  that  is  supposed  to  be  in 
the  group  gets  disconnected.  The  parameter  “-1”  indicates 
that  this  plan  will  run  forever  unless  interrupted.  It  will 
run  each  of  the  two  group  atoms,  one  after  the  other  .  The 
first  atom  moves  straight  ahead  until  the  front  is  not  clear, 
i.e.  obstacles  are  detected.  The  next  one  turns  the  robot 
until  the  front  is  clear,  i.e.  obstacles  are  avoided. 


Experiments  are  done  using  a  pair  of  Hilare-type  mobile 
robots  to  show  that  the  mapping  from  group  atoms  to 
shadow  atoms  is  necessary.  In  the  first  experiments,  the 
two  robots  will  run  the  sample  script  without  mapping 
the  group  atoms  into  shadow  atoms.  The  result  is  shown 
in  Figure  (1).  In  this  figure,  the  thick  lines  indicate  the 


Fig.  1.  Executing  the  Group  Plan  without  Formation  Control 

ground  trajectories  of  the  two  robots.  The  dashed  lines 
connected  the  two  robots  at  specific  time  to  show  their 
formation.  Positions  A,  B,  C  indicates  where  one  of  the 
robot  detects  obstacles.  We  can  see  that  the  robots  failed 
to  keep  in  the  starting  formation  after  a  short  while.  We 
find  that  the  main  reason  for  this  is  that  by  using  TCP/IP 
sockets  there  is  a  delay  of  on  average  about  200ms.  In  the 
second  experiment,  the  mappings  are  added.  A  simple  PI 
controller  is  implemented  within  the  mapping  process. 


Fig.  2.  Executing  the  Closed  Group  Plan  with  PI  Controllers 

As  one  can  see  in  Figure  (2),  the  controllers  have  caused 
the  robots  to  no  longer  track  straight  lines.  The  formation 
is  successfully  kept.  However,  the  performance  is  limited. 
Also  at  position  E,F  and  G  one  can  observe  errors  caused 
by  communication  delays. 


VI.  Summary 

In  this  paper  we  have  outlined  an  approach  to  the 
problem  of  formation  shape  control  using  the  theory  of 
Jacobi  coordinates  and  associated  block-diagonalization  of 
the  Lagrangian  dynamics  of  a  system  of  robots  modeled 
as  point  masses.  A  feedback  law  for  locally  stabilizing 
a  shape  of  interest  is  given.  The  approach  also  permits 
overall  advection  of  the  formation  in  the  Euclidean  group. 
Details  of  this  as  well  as  an  intrinsic  geometric  treatment 
of  our  ideas  will  appear  in  a  work  in  preparation.  We 
have  also  investigated  the  problem  of  devising  formation 
controllers  for  mobile  robots  in  software.  Proceeding 
from  a  motion  description  language  framework,  we  have 
shown  how  certain  specific  constructs  in  the  language 
MDLe  that  support  group  behavior  together  with  certain 
communication  primitives  that  support  effective  sharing 
of  sensor  data  enable  stable  coordination  of  a  set  of 
robots.  These  experimental  results  are  preliminary  and 
work  is  under  way  to  devise  light-weight  protocols  for 
robot  communication  that  reduce  the  effects  of  latency  in 
the  feedback  loop. 
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